import roslib; roslib.load_manifest('framer')
import rospy
import actionlib
import smach
from actionlib_msgs.msg import GoalStatus
from pr2_controllers_msgs.msg import PointHeadAction, PointHeadGoal
from framer.msg import Status
import tf

class MoveHead(smach.State):
   def __init__(self):
      smach.State.__init__(self, outcomes=['fail', 'success', 'abort'], input_keys=['picture_center'])

      self.tf = tf.TransformListener()
      rospy.loginfo('Waiting on tf transform from camera to head')
      self.tf.waitForTransform('wide_stereo_optical_frame', 'head_tilt_link', rospy.Time(), rospy.Duration(5.0))
      rospy.loginfo("Waiting on head trajectory client")
      self.client = actionlib.SimpleActionClient('head_traj_controller/point_head_action', PointHeadAction)
      self.client.wait_for_server()
      self.running = False
      rospy.loginfo("MoveHead initialized.")
      self.status_pub = rospy.Publisher('/framer/status', Status)

   def execute(self, userdata):
      self.status_pub.publish(Status(Status.MOVING_HEAD))
      if rospy.is_shutdown(): # Exiting gracefully when ctrl-c is pressed
         return 'abort'
      success = self.lookat(userdata.picture_center)
      if success:
         rospy.loginfo("Going to take your picture now. Smile!")
         return 'success'
      return 'fail'

   def lookat(self, point):
      if self.running: # Because this is in a state machine, this shouldn't happen. But just in case...
         rospy.loginfo("Already trying to look at something. Going to ignore most recent request")
         return False
      if point == None:
         rospy.loginfo("Something really strange happened, and goal point is null. Ignoring.")
         return False 
      self.running = True
      
      # Insert stuff to wait on gesture and preempt any other attempts at moving head

      g = PointHeadGoal()
      # Note - The head trajectory controller is silly, and assumes that you pass in something that is in the head_tilt_link frame. 
      #        This means it won't do the frame conversions for you.
      g.target = self.tf.transformPoint('head_tilt_link', point) 
      g.min_duration = rospy.Duration(2.0)
      rospy.loginfo("Attempting to look at {0},{1},{2} in {3}".format(point.point.x, point.point.y, point.point.z, point.header.frame_id))
      self.client.send_goal(g)
      self.client.wait_for_result(rospy.Duration(2.0))

      status = True
      if not self.client.get_state() == GoalStatus.SUCCEEDED:
         rospy.loginfo("Failed to look at face. Perhaps there was interference from another node. Do you have an instance of teleop running?")
         status = False 
      self.running = False
      return status 

